from machine import I2C,UART,Pin
from  Sentry3  import *
import time
import random

flag = 0
num = 5
x = 0
y = 0
i = 0

def result_output():
    global num, flag, x, y, i
    # Sentry3不主动返回检测识别结果，需要主控板发送指令进行读取。读取的流程：首先读取识别结果的数量，接收到指令后，Sentry3会刷新结果数据，如果结果数量不为零，那么主控再发送指令读取结果的相关信息。请务必按此流程构建程序。
    obj_num = Sentry3.GetValue(sentry3_vision_e.kVisionColor, sentry_obj_info_e.kStatus)
    if obj_num:
        print("Totally %d points: "%( obj_num ))
        for i in range(1, (obj_num + 1), 1):
            if (Sentry3.GetValue(sentry3_vision_e.kVisionColor,sentry_obj_info_e.kLabel,i)==color_label_e.kColorBlack):
              print("point%d: Black"%i)
            if (Sentry3.GetValue(sentry3_vision_e.kVisionColor,sentry_obj_info_e.kLabel,i)==color_label_e.kColorWhite):
              print("point%d: White"%i)
            if (Sentry3.GetValue(sentry3_vision_e.kVisionColor,sentry_obj_info_e.kLabel,i)==color_label_e.kColorRed):
              print("point%d: Red"%i)
            if (Sentry3.GetValue(sentry3_vision_e.kVisionColor,sentry_obj_info_e.kLabel,i)==color_label_e.kColorGreen):
              print("point%d: Green"%i)
            if (Sentry3.GetValue(sentry3_vision_e.kVisionColor,sentry_obj_info_e.kLabel,i)==color_label_e.kColorBlue):
              print("point%d: Blue"%i)
            if (Sentry3.GetValue(sentry3_vision_e.kVisionColor,sentry_obj_info_e.kLabel,i)==color_label_e.kColorYellow):            
              print("point%d: Yellow"%i)
            time.sleep(0.2)
        print("\n")
        

# 等待Sentry3完成操作系统的初始化。此等待时间不可去掉，避免出现Sentry3尚未初始化完毕主控器已经开发发送指令的情况
time.sleep(2)

# 选择UART或者I2C通讯模式，Sentry3出厂默认为I2C模式，短按模式按键可以切换
# 4种UART通讯模式：UART9600（标准协议指令），UART57600（标准协议指令），UART115200（标准协议指令），Simple9600（简单协议指令），
# 参看“简单协议指令”
# https://tosee.readthedocs.io/zh/latest/Common/Protocol/index.html#section-6
#########################################################################################################
# port = UART(2,rx=Pin(16),tx=Pin(17),baudrate=9600)
port = I2C(1,scl=Pin(22),sda=Pin(21),freq=400000)

# Sentry3通讯地址：0x60。如果I2C总线挂接多个设备，请避免出现地址冲突
Sentry3 = Sentry3(0x60)

err = Sentry3.begin(port)
print("Sentry3.begin: 0x%x"% err)


# 设置Sentry3两颗LED的颜色，可选：黑Close，白White，红Red，绿Green，黄Yellow，蓝Blue，紫Purple，青Cyan
# 三个参数依次为：检测识别到目标时的颜色，未发现目标的颜色，亮度
# 当环境光亮度不足时，可将LED的颜色全部设置为白White，并且将亮度值设置为最大15（范围：1-15）
# 无法分别设置两颗LED
Sentry3.LedSetColor(sentry_led_color_e.kLedBlue,sentry_led_color_e.kLedRed,1)


# 算法的默认参数，如果不用更改，则可以删除本行代码
Sentry3.SetParamNum(sentry3_vision_e.kVisionColor,1)
# 算法的默认参数，如果不用更改，则可以删除本行代码
Sentry3.SetParam(sentry3_vision_e.kVisionColor,[50, 50, 3, 4, 0],1)
 
 
# 1、算法介绍
# https://tosee.readthedocs.io/zh/latest/Sentry2/Vision/index.html#chapter-vision-color-index
# 2、参考视频
# https://www.bilibili.com/video/BV18K411Z7Gh/?spm_id_from=333.1387.upload.video_card.click&vd_source=a6a4a13d17ed26ee248472534a8ec3cc
# https://www.bilibili.com/video/BV1pP411s7zR/?spm_id_from=333.1387.homepage.video_card.click&vd_source=a6a4a13d17ed26ee248472534a8ec3cc
# 4、正常使用时，应由主控器发送指令控制Sentry2算法的开启与关闭，而非通过摇杆手动进行操作；
# 5、满足特定限制条件下，Sentry3可以并行运行多个识别算法
# 参看“算法列表”
# https://tosee.readthedocs.io/zh/latest/Sentry2/Hardware/index.html#section-3
err = Sentry3.VisionBegin(sentry3_vision_e.kVisionColor)
print("Sentry3.VisionBegin(sentry3_vision_e.kVisionColor):0x%x"% err)

#设置颜色识别算法5组参数
Sentry3.SetParamNum(sentry3_vision_e.kVisionColor,num)

#利用随机数，选择5个识别点的初始排列方式
flag = (random.randint(1, 4))
print("flag= %d"%flag)
print("\n")

while True:
    if (flag == 1):
        for i in range(1, (num + 1), 1):
            x = (round((i * (100 / (num + 1)))))
            y = 50
            Sentry3.SetParam(sentry3_vision_e.kVisionColor,[x, y, 6, 8, 0],i)
            time.sleep(0.1)
        result_output()
        flag += 1

    if (flag == 2):
        for i in range(1, (num + 1), 1):
            x = (round((i * (100 / (num + 1)))))
            y = 100-x
            Sentry3.SetParam(sentry3_vision_e.kVisionColor,[x, y, 6, 8, 0],i)
            time.sleep(0.1)
        result_output()
        flag += 1

    if (flag == 3):
        for i in range(1, (num + 1), 1):
            y = (round((i * (100 / (num + 1)))))
            x = 50
            Sentry3.SetParam(sentry3_vision_e.kVisionColor,[x, y, 6, 8, 0],i)
            time.sleep(0.1)
        result_output()
        flag += 1

    if (flag == 4):
        for i in range(1, (num + 1), 1):
            x = (round((i * (100 / (num + 1)))))
            y = x
            Sentry3.SetParam(sentry3_vision_e.kVisionColor,[x, y, 6, 8, 0],i)
            time.sleep(0.1)
        result_output()
        flag = 1